#include "particle2realflowUtil/smoothingGrid.H"
#include <iostream>
#include <fstream>
#include "particle2realflowUtil/marchingTet.H"
#include <float.h>
#include <time.h>
#include <sstream>
#include <string>
#include "particle2realflowUtil/CIsoSurface.h"
#include <iomanip>
#include <string> 
#include <cmath>

#include <Eigen/Dense>

#include "myFunctions.h"


using namespace std;
using namespace Eigen;


// convert the scalar field in grid to a mesh represented by Vertices & Faces
void dumpSurface(const SmoothingGrid &grid, float shrink, Eigen::MatrixXd & Vertices, Eigen::MatrixXi & Faces);


void SkinningParticles(const char *infname,float shrink, Eigen::MatrixXd & Vertices, 
	Eigen::MatrixXi & Faces, std::vector<SlVector3>& Particles, arguConfigure & gridInfo)
{
	unsigned int flags = 0;
	static int verboseFlag = 0;
	bool helpFlag = false;
	int iterLaplace = 15, iterBiharmonic = 100, redistanceFrequency = 50;
	double rmin = -DBL_MAX, rmax = -DBL_MAX, rinit = -DBL_MAX, velGain = 1.0,
		dtLaplace = -DBL_MAX, dtBiharmonic = -DBL_MAX, dtBiharmonicGain = 1.0;
	double maxStretch = 1.0, rratio = 4.0;

	std::vector<SlVector3> velocities;
	std::vector<double> radii;

	double h = gridInfo.h;

	if (verboseFlag) flags |= SmoothingGrid::VERBOSE;

	if (rmin == -DBL_MAX) {
		rmin = 0.86603 * h; // 0.5*sqrt(3)*h
							// there could be a stretch up to this amount, we want to make sure that particles
							// all touch at least one grid point, so make the radius bigger...
		if (flags & SmoothingGrid::NEIGHBOR_ANISOTROPY) {
			rmin *= sqrt(maxStretch);
		}
		else if (flags & SmoothingGrid::VELOCITY_ANISOTROPY) {
			rmin *= cbrt(maxStretch);
		}
	}

	if (rmax == -DBL_MAX) rmax = rratio * rmin;
	if (rinit == -DBL_MAX) rinit = 0.5*(rmin + rmax);
	else if (!(flags & SmoothingGrid::VARIABLE_RADIUS)) rinit *= rmin;
	if (dtLaplace == -DBL_MAX) dtLaplace = 0.1*h*h;
	if (dtBiharmonic == -DBL_MAX) dtBiharmonic = 0.01*dtBiharmonicGain*h*h*h*h;

	std::ifstream input(infname, std::ios::in | std::ios::binary);
	int nparticles;
	float minx = 100000.0f;
	float maxx = -100000.0f;
	float miny = 100000.0f;
	float maxy = -100000.0f;
	float minz = 100000.0f;
	float maxz = -100000.0f;
	float x, y, z;
	input.read((char*)&nparticles, sizeof(int));
	float fdummy;
	for (int i = 0; i < nparticles; i++)
	{
		input.read((char*)&x, sizeof(float));
		input.read((char*)&y, sizeof(float));
		input.read((char*)&z, sizeof(float));

		input.read((char*)&fdummy, sizeof(float));

		if (x < 2.0 && x > -1.0 && y < 2.0 && y > -1.0f && z < 2.0 && z > -1.0)
		{
			Particles.push_back(SlVector3(x, y, z));
		}

	}

	cout << "Number of particles: " << Particles.size() << endl;

	SmoothingGrid grid(h, rmin, rmax, rinit, velGain, maxStretch, flags, Particles, radii, velocities);

	grid.doLaplacianSmoothing(iterLaplace, dtLaplace, redistanceFrequency);

	grid.doBiharmonicSmoothing(iterBiharmonic, dtBiharmonic, redistanceFrequency);

	dumpSurface(grid, shrink, Vertices, Faces);

	radii.clear();
	velocities.clear();
}



void dumpSurface(const SmoothingGrid &grid, float shrink, Eigen::MatrixXd & Vertices, Eigen::MatrixXi & Faces)
{
	//// use libigl datatype Vertices&Fraces to replace vector type 

	//std::vector<SlTri> triangles; // triangles in polygon mesh
	//std::vector<SlVector3> meshPts; // points in polygon mesh

	CIsoSurface<float> mc;

	int nx = grid.nx;
	int ny = grid.ny;
	int nz = grid.nz;

	SlArray3D<float> phi(nz, ny, nx);
	for (int i = 0; i < phi.nx(); i++)
	{
		for (int j = 0; j < phi.ny(); j++)
		{
			for (int k = 0; k < phi.nz(); k++)
			{
				phi(i, j, k) = (float)(grid.phi(k, j, i));
			}
		}
	}

	//needs to be modified
	mc.GenerateSurface(phi.data, 0.0, nx - 1, ny - 1, nz - 2, (float)grid.h, (float)grid.h, (float)grid.h);

	Vertices.resize(mc.m_nVertices, 3);
	for (unsigned int i = 0; i<mc.m_nVertices; i++)
	{
		POINT3D& pt = mc.m_ppt3dVertices[i];
		//meshPts.push_back(SlVector3(pt[0] + grid.bbMin[0], pt[1] + grid.bbMin[1], pt[2] + grid.bbMin[2]));
		Vertices(i, 0) = pt[0] + grid.bbMin[0];
		Vertices(i, 1) = pt[1] + grid.bbMin[1];
		Vertices(i, 2) = pt[2] + grid.bbMin[2];
	}

	Faces.resize(mc.m_nTriangles, 3);
	for (unsigned int i = 0; i<mc.m_nTriangles; i++) {
		//triangles.push_back(SlTri(mc.m_piTriangleIndices[3 * i], mc.m_piTriangleIndices[3 * i + 1], mc.m_piTriangleIndices[3 * i + 2]));
		Faces(i, 0) = mc.m_piTriangleIndices[3 * i];
		Faces(i, 1) = mc.m_piTriangleIndices[3 * i + 1];
		Faces(i, 2) = mc.m_piTriangleIndices[3 * i + 2];
	}

}


// calculate singularities of each particles
// radius represents for the maximum searching area
// num represents for maximum number of particles which we take into account
void calculateParticleAnisotropy(const std::vector<SlVector3> & Particles, KDTree & particleKDTree,
									int num, double radius, Eigen::MatrixXd & ParticleAnisotropy)
{
	std::vector<int> neighborsIndex;
	Eigen::VectorXi numOfNeighborParticles(Particles.size());

	for (int i = 0; i < Particles.size(); ++i) {
		neighborsIndex.clear();
		SlVector3 slCurrentPoint = Particles.at(i);
		particleKDTree.neighbors(Particles, slCurrentPoint, num, radius, neighborsIndex);

		numOfNeighborParticles(i) = neighborsIndex.size();

		if (neighborsIndex.size() >= LeastAffectiveNeighbors) {	//we believe it won't work well if there is few particles
			std::vector<SlVector3> neighbors;
			std::vector<double> weight;

			//find all particle's location and weight
			for (int j = 0; j < neighborsIndex.size(); ++j) {
				SlVector3 slPoint = Particles.at( neighborsIndex.at(j) );
				double distance = mag(slPoint - slCurrentPoint);
				neighbors.push_back(slPoint);

				double cubicDistWeight = pow(distance / radius, 3);
				if (cubicDistWeight > 1.0) cubicDistWeight = 1.0;

				weight.push_back(1.0 - cubicDistWeight);
			}

			//compute sum of weight
			double sumWeight = 0.0;
			for (int j = 0; j < weight.size(); ++j) {
				sumWeight += weight.at(j);
			}

			SlVector3 slWeightedMeanP = SlVector3();
			for (int j = 0; j < neighbors.size(); ++j) {
				slWeightedMeanP += weight.at(j)*neighbors.at(j);
			}
			slWeightedMeanP /= sumWeight;
			Eigen::Vector3d meanP(slWeightedMeanP.x(), slWeightedMeanP.y(), slWeightedMeanP.z());

			// Compute covariance matrix
			Eigen::MatrixXd covarianceMatrix(3,3);
			covarianceMatrix.setZero();
			for (int j = 0; j < neighbors.size(); ++j) {
				SlVector3 slPoint = neighbors.at(j);
				Eigen::Vector3d point(slPoint.x(), slPoint.y(), slPoint.z());
				covarianceMatrix += weight.at(j) * ( (point - meanP)*(point-meanP).transpose() );
			}
			covarianceMatrix /= sumWeight;

			// SVD decomposition
			Eigen::Vector3d pSingularValue = 
								covarianceMatrix.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).singularValues();
			ParticleAnisotropy.row(i) = pSingularValue;
		}
		else {
			ParticleAnisotropy.row(i) = Eigen::Vector3d::Constant(0.0);
		}

	}

	std::cout << "Maximum number of neighbor points: "
		<< numOfNeighborParticles.maxCoeff() << endl;
	std::cout << "Minimum number of neighbor points: "
		<< numOfNeighborParticles.minCoeff() << endl;
	std::cout << "Average number of neighbor points: "
		<< (double)(numOfNeighborParticles.sum()) / (double)(numOfNeighborParticles.size()) << endl;

	std::cout << "Maximun particle singular value is " << ParticleAnisotropy.col(0).maxCoeff() << endl;

	std::cout << "Particles singular value computation accomplished!" << endl;
}